package chw412;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Stack;

import pacworld.Direction;

class Astar {
	//the goal
	private Point goal;
	//current location
	private Point current;
	//the open list
	private ArrayList<Point> openlist;
	//the close list
	private ArrayList<Point> closelist;
	//the map
	private map_state [][] map = null;
	//the pkg_direction to which an agent holds its package.
	//-1 means no package held.
	int pkg_direction = -1;
	
	//constructor, takes in the map as parameter
	//note that dir parameter is used for agents carrying packages to calculate the astar path.
	//dir indicate the pkg_direction from the agent to its package held.
	public Astar(map_state [][] map, Point goal, Point current, int dir){
		openlist = new ArrayList<Point>();
		closelist = new ArrayList<Point>();
		this.map = map;
		this.goal = goal;
		this.pkg_direction = dir;
		
		//Init current Point and its g and f cost value
		this.current = current;
		this.current.gcost = 0;
		this.current.fcost = this.current.gcost + this.current.heuristic_cost_estimate(goal);
		
		//Initially, current is the only point in the open list
		openlist.add(current);
	}
	
	public boolean in_openlist(Point p){
		int index = Collections.binarySearch(openlist, p);
		if(index >= 0){
			return true;
		}else
			return false;
	}
	
	public boolean in_closelist(Point p){
		int index = Collections.binarySearch(closelist, p);
		if(index >= 0){
			return true;
		}else
			return false;
	}
	
	public Point find_mincost_point(){
		int min = Integer.MAX_VALUE;
		Point minP = null;
		
		for(Point p : openlist){
			if(p.fcost < min){
				min = p.fcost;
				minP = p;
			}
		}
		
		return minP;
	}
	
	/*
	 * This predicate "defines" whether there is an obstacle in the direction dir of point p.
	 * 
	 * If no package is held by the agent, obviously the agent can go any of the four pkg_directions if the locations there are clear.
	 * However, if an agent holds an package, things become different and complicated.
	 */
	boolean has_obstacle(Point p, int dir){
		int chx, chy, chm, chn;
		if(pkg_direction == -1)	{		//the agent holds no package, so this predicate works normally as before.
			chx = p.getX() + Direction.DELTA_X[dir];
			chy = p.getY() + Direction.DELTA_Y[dir];
			return !(chx >= 0 && chx < map.length && chy >= 0 && chy < map.length) || map[chx][chy] != map_state.Clear;
		}else{
			if(pkg_direction == dir) {	//the position of package is the same with the next candidate position.
				chx = p.getX() + Direction.DELTA_X[dir]*2;
				chy = p.getY() + Direction.DELTA_Y[dir]*2;
				return !(chx >= 0 && chx < map.length && chy >= 0 && chy < map.length) || map[chx][chy] != map_state.Clear;
			}
			else if(pkg_direction == (dir + 2) % 4) {	//the position of package is opposite to the next candidate position.
				chx = p.getX() + Direction.DELTA_X[dir];
				chy = p.getY() + Direction.DELTA_Y[dir];
				return !(chx >= 0 && chx < map.length && chy >= 0 && chy < map.length) || map[chx][chy] != map_state.Clear;
			}
			else if(pkg_direction == (dir + 1) % 4 || pkg_direction == (dir + 3) % 4) {
				chx = p.getX() + Direction.DELTA_X[dir];
				chy = p.getY() + Direction.DELTA_Y[dir];
				chm = chx + Direction.DELTA_X[pkg_direction];
				chn = chy + Direction.DELTA_Y[pkg_direction];
				return !(chx >= 0 && chx < map.length && chy >= 0 && chy < map.length) || 
						!(chm >= 0 && chm < map.length && chn >= 0 && chn < map.length) || 
							map[chx][chy] != map_state.Clear || map[chm][chn] != map_state.Clear; 
			}else{
				assert(false);
				return true;
			}
		}
	}
	
	//open the point and add potential candidate to the open list
	void open(Point p){
		openlist.remove(p);
		assert(map[p.getX()][p.getY()] == map_state.Clear);
		
		for(int dir = 0; dir <= 3; ++dir){
			int chx = p.getX() + Direction.DELTA_X[dir];
			int chy = p.getY() + Direction.DELTA_Y[dir];
			if(!(chx >= 0 && chx < map.length && chy >= 0 && chy < map.length))	//chx, chy is not illegal, so we do nothing but continue.
				continue;
			
			//chx, chy is now illegal; we can create the child point now.
			Point child = new Point(chx, chy, p);
			
			int tg = p.gcost + p.dist(child);
			int tf = tg + child.heuristic_cost_estimate(goal);
			
			if(has_obstacle(p, dir) || (in_closelist(child) && tf >= child.fcost))
					continue;
			
			if(!in_openlist(child) || tf < child.fcost){
				child.gcost = tg;
				child.fcost = tf;
				if(!in_openlist(child))
					openlist.add(child);
			}
		}
		
		Collections.sort(openlist);
	}
	
	void close(Point p){
		closelist.add(p);
		Collections.sort(closelist);
	}
	
	//search a path from the current point to the goal point
	public boolean astar_search(){
		//if open list is empty, we failed to find a path from current point to the target point.
		if(openlist.isEmpty())
			return false;
		//if goal is in open list, a path has been found and our search is finished.
		if(in_openlist(goal))
			return true;
		
		Point min = find_mincost_point();
		open(min);
		close(min);
		return astar_search();
	}
	
	//if search successfully completes, we generate a path from current point to the goal point
	ArrayList<Integer> construct_path(){
		//the generated path after the algorithm successfully completes.
		ArrayList<Integer> path = new ArrayList<Integer>();
		
		int index = Collections.binarySearch(openlist, goal);
		if(index < 0)	//we have not found the path yet.
			return null;
		Stack<Point> sp = new Stack<Point>();
		Point g = openlist.get(index);
		
		while(g != current){
			sp.push(g);
			g = g.parent;
		}
		
		assert(g == current);
		
		while(!sp.isEmpty()){
			Point p = sp.pop();
			int delta_x = p.getX() - g.getX();
			int delta_y = p.getY() - g.getY();
			if(delta_x == 0 && delta_y == -1)
				path.add(Direction.NORTH);
			else if(delta_x == 1 && delta_y == 0)
				path.add(Direction.EAST);
			else if(delta_x == 0 && delta_y == 1)
				path.add(Direction.SOUTH);
			else if(delta_x == -1 && delta_y == 0)
				path.add(Direction.WEST);
			else
				assert(false);
			g = p;
		}
		
		return path;
	}
};